#include "damiao.h"
#include "dji.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robotdef.h"
#include "rtthread.h"
#include "rttypes.h"
#include "imu.h"
#include "dm_imu.h"

#define LOG_TAG              "gimcmd"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

#if defined (GIMBAL_BOARD)
    #if defined (SENTRY_MODE)
    static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
    static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
    static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
    static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

    DJIMotor_t *big_yaw = RT_NULL;
    DJIMotor_t *small_yaw = RT_NULL; 
    DMMOTOR_t *pitch_motor = RT_NULL; 
    void gimbal_init()
    {
        Motor_Init_Config_s yaw_config = {
            .offline_manage_motor ={
                .event = OFFLINE_GIMBAL_YAW,
                .enable = OFFLINE_ENABLE,
                .error_level =OFFLINE_ERROR_LEVEL,
                .beep_times = 1,
                .offline_time =500,
            },
            .can_init_config = {
                .can_handle = &can2_bus,
                .tx_id = 1,
            },
            .controller_param_init_config = {
                .other_angle_feedback_ptr = &dm_imu.YawTotalAngle,
                .other_speed_feedback_ptr = &dm_imu.gyro[2] ,
                .lqr_config ={
                    .K ={22.36f,4.05f},
                    .output_max = 2.223,
                    .output_min =-2.223,
                    .state_dim = 2,
                    .compensation_type =COMPENSATION_NONE,
                }
            },
            .controller_setting_init_config = {
                .control_algorithm = CONTROL_LQR,
                .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
                .angle_feedback_source =OTHER_FEED,
                .speed_feedback_source =OTHER_FEED,
                .outer_loop_type = ANGLE_LOOP,
                .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            },
            .motor_type = GM6020_CURRENT,
        };
        big_yaw = DJIMotorInit(&yaw_config);

        Motor_Init_Config_s small_yaw_config = {
            .offline_manage_motor ={
                .event = OFFLINE_GIMBAL_SMALL_YAW,
                .enable = OFFLINE_ENABLE,
                .error_level =OFFLINE_ERROR_LEVEL,
                .beep_times = 2,
                .offline_time =500,
            },
            .can_init_config = {
                .can_handle = &can1_bus,
                .tx_id = 1,
            },
            .controller_param_init_config = {
                .other_angle_feedback_ptr = &INS.YawTotalAngle,
                .other_speed_feedback_ptr = &INS.Gyro[2] ,
                .lqr_config ={
                    .K ={17.32f,1.0f},
                    .output_max = 2.223,
                    .output_min =-2.223,
                    .state_dim = 2,
                    .compensation_type =COMPENSATION_NONE,
                }
            },
            .controller_setting_init_config = {
                .control_algorithm = CONTROL_LQR,
                .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
                .angle_feedback_source =OTHER_FEED,
                .speed_feedback_source =OTHER_FEED,
                .outer_loop_type = ANGLE_LOOP,
                .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            },
            .motor_type = GM6020_CURRENT,
        };
        small_yaw =DJIMotorInit(&small_yaw_config);
        // PITCH
        Motor_Init_Config_s pitch_config = {
            .offline_manage_motor ={
                .event = OFFLINE_GIMBAL_PITCH,
                .enable = OFFLINE_ENABLE,
                .error_level =OFFLINE_ERROR_LEVEL,
                .beep_times = 3,
                .offline_time =10000,
            },
            .can_init_config = {
                .can_handle = &can1_bus,
                .tx_id = 0X23,
                .rx_id = 0X206,
            },
            .controller_param_init_config = {
                .other_angle_feedback_ptr = &INS.Pitch,
                .other_speed_feedback_ptr = &INS.Gyro[0],
                .lqr_config ={
                    .K ={44.7214f,3.3411f}, //28.7312f,2.5974f
                    .output_max = 7,
                    .output_min = -7,
                    .state_dim = 2,
                    .compensation_type =COMPENSATION_GRAVITY,
                    .arm_length = 0.09,
                    .gravity_force = 16,
                }
            },
            .controller_setting_init_config = {
                .control_algorithm = CONTROL_LQR,
                .feedback_reverse_flag =FEEDBACK_DIRECTION_REVERSE,              
                .angle_feedback_source =OTHER_FEED,
                .speed_feedback_source =OTHER_FEED,
            },
            .motor_type = DM4310};
        pitch_motor = DMMotorInit(&pitch_config,MIT_MODE);

        gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
        gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    }
    #endif

static void gimbal_thread_entry(void *parameter)
{   
    rt_thread_mdelay(2000);
    gimbal_init();
    while (1)
    {
        SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
        if (offline_manage[OFFLINE_GIMBAL_YAW].online_state == STATE_ONLINE 
         && offline_manage[OFFLINE_GIMBAL_SMALL_YAW].online_state == STATE_ONLINE
         && offline_manage[OFFLINE_GIMBAL_PITCH].online_state == STATE_ONLINE) 
        {
            switch (gimbal_cmd_recv.gimbal_mode) 
            {
                case GIMBAL_ZERO_FORCE:
                    DJIMotorStop(big_yaw);
                    DJIMotorStop(small_yaw);
                    DMMotorStop(pitch_motor); 
                    break;
                case GIMBAL_KEEPING_SMALL_YAW:
                {
                    DJIMotorEnable(big_yaw);
                    DJIMotorEnable(small_yaw);
                    DMMotorEnable(pitch_motor); 

                    DJIMotorSetRef(big_yaw,gimbal_cmd_recv.yaw);
                    DMMotorSetRef(pitch_motor, SMALL_YAW_PITCH_HORIZON_ANGLE); 

                    // 计算相对角度
                    float small_yaw_offset = INS.YawTotalAngle - small_yaw->measure.total_angle; 
                    DJIMotorSetRef(small_yaw, small_yaw_offset);
                    break;
                }
                case GIMBAL_KEEPING_BIG_YAW:
                {
                    DJIMotorEnable(big_yaw);
                    DJIMotorEnable(small_yaw);
                    DMMotorEnable(pitch_motor); 
                    float small_yaw_offset = INS.YawTotalAngle - small_yaw->measure.total_angle; 
                    DJIMotorSetRef(big_yaw,gimbal_cmd_recv.yaw);
                    DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
                    DJIMotorSetRef(small_yaw, gimbal_cmd_recv.small_yaw + small_yaw_offset);
                    break;
                }
                case GIMBAL_AUTO_MODE:
                    DJIMotorEnable(big_yaw);
                    DJIMotorEnable(small_yaw);
                    DMMotorEnable(pitch_motor); 

                    DJIMotorSetRef(big_yaw,gimbal_cmd_recv.yaw);
                    DMMotorSetRef(pitch_motor, SMALL_YAW_PITCH_HORIZON_ANGLE); 

                    // 计算相对角度
                    float small_yaw_offset = INS.YawTotalAngle - small_yaw->measure.total_angle; 
                    DJIMotorSetRef(small_yaw, small_yaw_offset);
                    break;
                case GIMBAL_GYRO_MODE:
                default:
                    break;
            }
        }
        else
        {
            DJIMotorStop(big_yaw);
            DJIMotorStop(small_yaw);
            DMMotorStop(pitch_motor);
        }

            // 设置反馈数据,主要是imu和yaw的ecd
        if (offline_manage[OFFLINE_GIMBAL_YAW].online_state ==STATE_ONLINE)
        {
            gimbal_feedback_data.yaw_motor_single_round_angle = big_yaw->measure.angle_single_round;
            // 推送消息
            PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
        }else
        {
            gimbal_feedback_data.yaw_motor_single_round_angle = YAW_ALIGN_ANGLE * ECD_ANGLE_COEF_DJI;
            // 推送消息
            PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
        }
        rt_thread_mdelay(2);
    }

}
int gimbal_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("gimbal", gimbal_thread_entry, RT_NULL, 4096, 18, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("gimbal thread init success!\n");

    return ret;
}

INIT_APP_EXPORT(gimbal_thread_entry_init);
#endif 